#include "precompiled.h"
#include "Physics_Character.h"
#include "Physics_Scenery.h"
#include "PhysicsCommon.h"
#include "Player.h"
#include "Sound_Manager.h"
#include "ParticleManager.h"
#include "World_Manager.h"


Physics_Character::Physics_Character(void)
{
}


Physics_Character::~Physics_Character(void)
{
	m_characterRigidBody->removeReference();

	m_standShape->removeReference();
	m_crouchShape->removeReference();

	delete m_characterContext;

	delete m_previousGround;
}

void Physics_Character::Create( hkpWorld* m_world ){

	m_world->markForWrite();
	m_world->lock();
	//	Create a character rigid body object
	{
		// Construct a shape
		hkVector4 vertexA(0, 0.4f, 0);
		hkVector4 vertexB(0, -0.4f, 0);

		// Create a capsule to represent the character standing
		m_standShape = new hkpCapsuleShape(vertexA, vertexB, 3);

		// Create a capsule to represent the character crouching
		// Note that we create the smaller capsule with the base at the same position as the larger capsule.		
		vertexA.setZero4();
		m_crouchShape = new hkpCapsuleShape(vertexA, vertexB, 3);


		// Construct a character rigid body
		hkpCharacterRigidBodyCinfo info;
		info.m_mass = 100.0f;
		info.m_shape = m_standShape;

		
		info.m_maxForce = 1000.0f;
		info.m_up = hUP;
		info.m_position.set(-20.0f, 6.0f, 50.0f);
		info.m_maxSlope = 70.0f * HK_REAL_DEG_TO_RAD;
		

		m_characterRigidBody = new hkpCharacterRigidBody( info );
		{
			hkpCharacterRigidBodyListener* listener = new hkpCharacterRigidBodyListener();
			m_characterRigidBody->setListener( listener );
			listener->removeReference();
		}
		m_world->addEntity( m_characterRigidBody->getRigidBody() );

	}

	// Create the Character state machine and context
	{
		hkpCharacterStateManager* manager = new hkpCharacterStateManager();

		hkpCharacterStateOnGround *gstate = new hkpCharacterStateOnGround();
		gstate->setSpeed( 20 );
		manager->registerState( gstate,	HK_CHARACTER_ON_GROUND);
		gstate->removeReference();

		hkpCharacterStateInAir *astate = new hkpCharacterStateInAir();
		manager->registerState( astate,	HK_CHARACTER_IN_AIR);
		astate->removeReference();

		hkpCharacterStateJumping *jstate = new hkpCharacterStateJumping();
		jstate->setJumpHeight( 20 );
		manager->registerState( jstate,	HK_CHARACTER_JUMPING);
		jstate->removeReference();

		prev_state = HK_CHARACTER_IN_AIR;
		m_characterContext = new hkpCharacterContext( manager, HK_CHARACTER_IN_AIR );
		manager->removeReference();

		// Set character type
		m_characterContext->setCharacterType(hkpCharacterContext::HK_CHARACTER_RIGIDBODY);
	}

	// Initialize hkpSurfaceInfo for old ground info holding
	m_previousGround = new hkpSurfaceInfo();
	m_framesInAir = 0;

	m_world->unlock();
	m_world->unmarkForWrite();
}

void Physics_Character::Update( hkpWorld* m_world, const hkReal& m_timestep, Player* player ){


	{
		m_world->lock();

		//	Get user input data
		hkReal posX = 0.f;
		hkReal posZ = 0.f;
		{
			/*CharacterUtils::getUserInputForCharacter(m_env, deltaAngle, posX, posY);
			m_currentAngle += deltaAngle;
			m_currentOrient.setAxisAngle(UP, m_currentAngle);*/

			if( player != 0 ){
				if( glfwGetKey('W') ) posZ = 1;
				if( glfwGetKey('S') ) posZ = -1;
				if( glfwGetKey('A') ) posX = -1;
				if( glfwGetKey('D') ) posX = 1;

				/*printf("player controller move :(%f, %f)\n",
				posX,posY);*/
			}
		}

		HK_TIMER_BEGIN( "set character state", HK_NULL );

		hkpCharacterInput input;
		hkpCharacterOutput output;
		{
			input.m_inputLR = posX;
			input.m_inputUD = posZ;

			input.m_wantJump =  glfwGetKey(GLFW_KEY_SPACE);
			
			input.m_atLadder = false;

			input.m_up = hUP;
			input.m_forward = hFORWARD;
			
			if( player != 0 ){
				glm::vec3 forward = player->getDirection();
				input.m_forward.set( -forward.x, 0, -forward.z, 0 );
			}


			hkStepInfo stepInfo;
			stepInfo.m_deltaTime = m_timestep;
			stepInfo.m_invDeltaTime = 1.0f/m_timestep;

			input.m_stepInfo = stepInfo;

			input.m_characterGravity = hCHARACTER_GRAVITY;
			input.m_velocity = m_characterRigidBody->getRigidBody()->getLinearVelocity();
			input.m_position = m_characterRigidBody->getRigidBody()->getPosition();

			m_characterRigidBody->checkSupport(stepInfo, input.m_surfaceInfo);

			// Change character rigid body color according to its state
			if( input.m_surfaceInfo.m_supportedState == hkpSurfaceInfo::SUPPORTED )
			{
				HK_SET_OBJECT_COLOR( (hkUlong) m_characterRigidBody->getRigidBody()->getCollidable(), hkColor::rgbFromChars( 0, 255, 0, 100 ) );
			}
			else
			{
				HK_SET_OBJECT_COLOR( (hkUlong) m_characterRigidBody->getRigidBody()->getCollidable(), hkColor::BLUE );
			}

			HK_TIMER_END();
		}

		// Apply the character state machine
		{
			HK_TIMER_BEGIN( "update character state", HK_NULL );

			m_characterContext->update( input, output );

			HK_TIMER_END();
		}

		//Apply the player character controller
		{
			HK_TIMER_BEGIN( "simulate character", HK_NULL );

			// Set output velocity from state machine into character rigid body
			m_characterRigidBody->setLinearVelocity(
				output.m_velocity, m_timestep);

			HK_TIMER_END();

			m_world->unlock();
		}

		// Update our player/camera position.
		if( player != 0 ){
			hkVector4 p = m_characterRigidBody->getPosition();
			float px = static_cast<float>(p(0));
			float py = static_cast<float>(p(1));
			float pz = static_cast<float>(p(2));

			/*printf("Updating player position to :(%f, %f, %f)\n",
				px,py,pz );*/

			player->setPosition( px,py,pz );
		}

		// Display state
		{
			hkpCharacterStateType state = m_characterContext->getState();
			const char * stateStr;

			if( state != prev_state ){
				switch (state)
				{
				case HK_CHARACTER_ON_GROUND:
					stateStr = "On Ground";	break;
				case HK_CHARACTER_JUMPING:
					stateStr = "Jumping"; 
					SoundManager::OrderASound( "jump", false );
					break;
				case HK_CHARACTER_IN_AIR:
					stateStr = "In Air"; break;
				default:
					stateStr = "Other";	break;
				}

				printf("State : %s\n", stateStr);
				prev_state=state;
			}

		}
		
		//
		// Handle crouching (only for capsule)
		//
		{
			m_world->lock();
			hkBool wantCrouch = glfwGetKey( GLFW_KEY_LCTRL );

			hkBool isCrouching = (m_characterRigidBody->getRigidBody()->getCollidable()->getShape() == m_crouchShape);


			// We want to stand
			if (isCrouching && !wantCrouch)
			{
				m_characterRigidBody->getRigidBody()->setShape(m_standShape);
			}

			// We want to crouch
			if (!isCrouching && wantCrouch)
			{
				m_characterRigidBody->getRigidBody()->setShape(m_crouchShape);
			}

			m_world->unlock();
		}

		// Cast rays from our gun to shoot it.
		hkUint32 hit_id = 0;
		//cast_ray_gun( m_world, player->get_gunButt(), player->get_gunHead(), &hit_id );
		cast_ray_gun( m_world, player->get_lookFrom(), player->get_lookTo(), &hit_id );
	}


}

hkVector4 Physics_Character::GetPosition(){
	return m_characterRigidBody->getPosition();
}


	bool Physics_Character::cast_ray_gun( hkpWorld* world, glm::vec3 butt, glm::vec3 head, 
		hkUint32 *id ){
		 bool result = false;

		 float dist = 1000.0f;

		 world->lock();
		 // The start and end points are both specified in World Space as we are using the world castRay() method
		 // to fire the ray.
		 hkpWorldRayCastInput input;
		 input.m_from.set( butt.x, butt.y, butt.z );
		 input.m_to.set( head.x, head.y, head.z );

		 hkpClosestRayHitCollector output;
		 world->castRay(input, output );

		 // To visualize the raycast we make use of a macro defined in "hkDebugDisplay.h" called HK_DISPLAY_LINE.
		 // The macro takes three parameters: a start point, an end point and the line color.
		 // If a hit is found we display a RED line from the raycast start point to the point of intersection and mark that
		 // point with a small RED cross. The intersection point is calculated using: startWorld + (result.m_mindist * endWorld).
		 //
		 // If no hit is found we simply display a GREY line between the raycast start and end points.

		 if( output.hasHit() )
		 {
			 hkVector4 intersectionPointWorld;
			 intersectionPointWorld.setInterpolate4( input.m_from, input.m_to, output.getHit().m_hitFraction );

			 if( VISUALIZE_GUN_RAYCAST ){
				 //printf("gun_to: (%f,%f,%f)\n", input.m_to(0), input.m_to(1), input.m_to(2) );
				 HK_DISPLAY_LINE( input.m_from, intersectionPointWorld, hkColor::RED);
				 HK_DISPLAY_ARROW( intersectionPointWorld, output.getHit().m_normal, hkColor::CYAN);
			 }

			 hkpRigidBody *hrb = static_cast<hkpRigidBody*>(output.getHit().m_rootCollidable->getOwner());
			 
			 Manage_HitScenery( hrb->m_uid, intersectionPointWorld );

			 result=true;
		 }
		 else
		 {
			 if( VISUALIZE_GUN_RAYCAST ){
				 // Otherwise draw as GREY
				 //printf("gun_to: (%f,%f,%f)\n", input.m_to(0), input.m_to(1), input.m_to(2) );
				 HK_DISPLAY_LINE(input.m_from, input.m_to, hkColor::rgbFromChars(200, 200, 200));
			 }
		 }


		 if( VISUALIZE_GUN_RAYCAST ){
			 float sscale = 10.0f;
			 HK_DISPLAY_STAR( hkVector4( butt.x, butt.y, butt.z, 0), sscale, hkColor::WHITE );
			 HK_DISPLAY_STAR( hkVector4( head.x, head.y, head.z, 0), sscale, hkColor::GREEN );
		 }

		 world->unlock();

		 return result;
	 }


	void Physics_Character::Manage_HitScenery( hkUint32 muid, hkVector4 &intersectionPointWorld ){
		/*Physics_Scenery* ptr_scenery=0;
		ptr_scenery = RigidBodyGameObjectMap::get_scenery(muid);*/

		if( glfwGetMouseButton( GLFW_MOUSE_BUTTON_LEFT) ){
			ParticleManager::UpdateParticles_StartEnd( "dbg", World_Manager::player->get_gunButt(), 
				glm::vec3(intersectionPointWorld(0),intersectionPointWorld(1),intersectionPointWorld(2)) );
		}

		/*if( ptr_scenery == 0 )
			return;

		if( glfwGetMouseButton( GLFW_MOUSE_BUTTON_LEFT) )
			ptr_scenery->setDoFlash();*/
	}

